Metody i algorytmy planowania ruchu - laboratorium

Lab 7 - Biblioteka Open Motion Planning Library w Robot Operating System 2

1. Wprowadzenie

Celem zajęć jest zapoznanie się z biblioteką Open Motion Planning Library, która udostępnia implementację wielu popularnych algorytmów planowania ruchu. Dokumentacja biblioteki znajduje się na stronie: http://ompl.kavrakilab.org/

Lista dostępnych algorytmów planowania ruchu dostępna jest tutaj: http://ompl.kavrakilab.org/planners.html

W Robot Operating System modułem wykorzystującym bibliotekę OMPL jest moduł MoveIt: https://moveit.ros.org/

Warto również zapoznać się z przykładami zastosowania biblioteki OMPL: http://ompl.kavrakilab.org/gallery.html
i porównaniem poszczególnych algorytmów dla różnych problemów planowania ruchu: http://plannerarena.org/

Źródła biblioteki znajdują się na Github: https://github.com/ompl/ompl

Można tam znaleźć wiele przykładów planowania ruchu w C++ i Python.

⚠️ Pamiętaj, aby w każdym nowym terminalu zanim rozpoczniesz pracę skonfigurować środowisko ROS komendą
source /opt/ros/humble/setup.bash lub source install/setup.bash

2. Przygotowanie paczki w ROS

Zainstaluj niezbędne biblioteki w systemie:

sudo apt-get install ros-humble-nav2-map-server ros-humble-nav2-lifecycle-manager libompl-dev ros-humble-moveit-planners-ompl

Pobierz paczkę z zadaniami z repozytorium github:

cd ~/ros2_ws/src
git clone https://github.com/dominikbelter/ompl_example_2d
git clone --branch humble https://github.com/dominikbelter/example_maps

Następnie skompiluj pobraną paczkę i odśwież przestrzeń roboczą:

cd ~/ros2_ws
colcon build --symlink-install
source install/setup.bash

3. OMPL

Główny program znajduje się w pliku ompl_example_2d.cpp

Klasa Planner2D wykorzystuje bibliotekę OMPL do zaplanowania ruchu pomiędzy dwoma punktami. Pozycja początkowa robota, przestrzeń poszukiwań zdefiniowane są w metodzie configure(void).

Za planowanie ruchu odpowiedzialna jest metoda planPath. W tej metodzie tworzony jest planer OMPL i definiowany jest problem. Do sprawdzenia stanu robota służy funkcja isStateValid(const ob::State *state). Sprawdza ona czy aktualny stan (w naszym przypadku (x,y) jest dopuszczalny). W podstawowej wersji przeszkoda zdefiniowana jest jako prostokąt:

// define the obstacle
if (coordX->values[0]<5.1 && coordX->values[0]>5.0){
        if (coordY->values[0]<4.0 && coordY->values[0]>-5.0){
                return false;
        }
}

Uzyskana ścieżka jest konwertowana do typu ROSa w metodzie extractPath.

W naszym przykładzie wykorzystujemy bibliotekę OMPL. ROS służy nam tylko do wizualizacji i wymiany danych pomiędzy modułami. Węzeł ROS tworzony jest w pliku ompl_example_2d.cpp. W tym pliku subskrybowany jest topik /map (wykorzystany później w zadaniach) i publikowana jest zaplanowana ścieżka do topika /ompl_example_2d/planned_path. Planowanie ruchu odbywa się w pętli ROS co 1 sekundę.

W paczce ompl_example_2d znajduje się również plik launch uruchamiający oprogramowanie i niezbędne dodatki (map_server wczytujący przykładową mapę zajętości, RViz) i węzeł do planowania ruchu ompl_example_2d_node.

Uruchomienie przykładu następuje po wykonaniu poleceń:

ros2 launch ompl_example_2d ompl_example_2d.launch.py

Powinna pojawić się wizualizacja zaplanowanej ścieżki, która zmienia się co 1 sekundę (wizualizacja mapy zajętości powinna zostać wyłączona ponieważ nie jest brana pod uwagę podczas planowania ruchu):

Na rysunku czerwoną linią zaznaczono przeszkodę, która jest omijana przez planer ruchu.

🔨 Zadanie 3.1

Zmodyfikuj metodę isStateValid, tak żeby podczas planowania ruchu pod uwagę była brana wczytana mapa zajętości. Przykładowy wynik:

Pobranie mapy przez węzeł zajmuje trochę czasu - musi ona zostać pobrana z topika. Zanim to się stanie, pole occupancyMap.data ma rozmiar 0. Odwołanie do tej struktury powinno odbywać się dopiero po sprawdzeniu rozmiaru mapy (pole occupancyMap.info.width i occupancyMap.info.height)

🔨 Zadanie 3.2

Zmodyfikuj kod, tak żeby do planowania ruchu wykorzystana została metoda LazyPRMstar. Przykładowy wynik:

🔨 Zadanie 3.3

Zmodyfikuj kod, tak żeby planer uwzględniał rozmiar robota (10x10 komórek mapy zajętości) i poruszał się również po komórkach nieznanych (-1).

🔨 Zadanie 3.4 (dla chętnych 💪)

Zmodyfikuj kod, tak żeby planer uwzględniał orientację robota podczas planowania ruchu:
https://ompl.kavrakilab.org/workingWithStates.html

Wykorzystaj metodę setMotionValidator: https://ompl.kavrakilab.org/stateValidation.html
żeby ograniczyć możliwą zmianę orientacji w jednym kroku maksymalnie o 30 stopni.